function Y = geodesic_generic_best_new(clusters, occupancy)

DIM = size(clusters.covars, 1) / 2;
Q = clusters.ncentres;
x = 1:DIM;
dx = (DIM+1):(DIM+DIM);

T = size(occupancy, 1);
I = eye(DIM);
Z = zeros(DIM);

%allocate space for mapping matrices
F1 = spalloc(3*DIM*(T+1) - 2*DIM, DIM*(T+1), 3*DIM*(T+1));
F1(1:2*DIM, 1:2*DIM) = [I Z; 
                        Z I];
for t=1:T-1
    rows = (t-1)*3*DIM + 2*DIM + (1:3*DIM); cols = (t-1)*DIM + (1:3*DIM);
    F1(rows, cols) = [I Z Z; 
                      Z I Z; 
                      Z Z I];
end
F1((T-1)*3*DIM + 2*DIM + (1:2*DIM), (T-1)*DIM + (1:2*DIM)) = [I Z; 
                                                              Z I];

for i = 1:clusters.ncentres
    K{i} = inv(clusters.covars(:, :, i));

    A{i} = computeA(K{i}, occupancy(:, i), x, dx, DIM, T);
    B{i} = computeB(K{i}, clusters.centres(i, x)', clusters.centres(i, dx)', occupancy(:, i), x, dx, DIM, T);

    if i == 1
        AA = A{i} * F1;
        BB = B{i};
    else
        AA = AA + A{i} * F1;
        BB = BB + B{i};
    end
end

Y = AA \ BB';
Y = reshape(Y, DIM, T+1)';



function AA = computeA(K, gamma, x, dx, DIM, T)

A = K(x, x);  B = K(x, dx);
C = K(dx, x); D = K(dx, dx);
AA = spalloc(DIM*(T+1), 3*DIM*(T+1) - 2*DIM, 3*DIM*DIM*(T+1));
AA(1:DIM, 1:2*DIM) = [ gamma(1) * D, ...
                      -gamma(1) * (C + D)];
for t =1:T-1
    rows = t*DIM + (1:DIM);     cols = (t-1)*3*DIM + 2*DIM + (1:3*DIM);
    AA(rows, cols) = [-gamma(t) * (B + D), ...
                       gamma(t) * (A + B + C + D) + gamma(t+1) * D, ...
                      -gamma(t+1) * (C + D)];
end
AA(T*DIM + (1:DIM), (T-1)*3*DIM + 2*DIM + (1:2*DIM)) = [-gamma(end) * (B + D), ...
                                                         gamma(end) * (A + B + C + D)];


function BB = computeB(K, mu, dmu, gamma, x, dx, DIM, T)

A = K(x, x);  B = K(x, dx);
C = K(dx, x); D = K(dx, dx);
BB(1:DIM) = - gamma(1) * (C * mu + D * dmu);                                 
for t=1:T-1
    BB(t*DIM+(1:DIM)) = gamma(t) * ((A + C) * mu + (B + D) * dmu) - gamma(t+1) * (C * mu + D * dmu);    
end
BB(T*DIM+(1:DIM)) = gamma(end) * ((A + C) * mu + (B + D) * dmu);  
